﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace LocalizationTest
{
  public abstract class Sensor
  {
    public abstract Measurement PredictForSimulation(WorldState worldState, Map map);
    public abstract Measurement PredictPerfect(WorldState worldState, Map map);

    internal virtual void SimulateMeasurement(WorldState worldState, Map map, ActionAndObservationHistory actionAndObservationHistory)
    {
      Measurement performedMeasurement = PredictForSimulation(worldState, map);
      actionAndObservationHistory.Add(performedMeasurement);
    }
    
    public virtual double GetMeasurementDeviation(WorldState worldState, Map map, Measurement measurement)
    {
      Measurement prediction = measurement.sensor.PredictPerfect(worldState, map);
      double difference = prediction.GetDifference(measurement);
      return difference;
    }
    
    public abstract double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement);

    public BeliefProbabilityDistribution UpdateBelief(BeliefProbabilityDistribution beliefAfterMotionUpdate, Map map, Measurement measurement)
    {
      BeliefProbabilityDistribution newBelief = new BeliefProbabilityDistribution();

      foreach (WorldState worldState in beliefAfterMotionUpdate.StatesWithProbability.Keys)
      {
        newBelief.AddValue(worldState,
            beliefAfterMotionUpdate.StatesWithProbability[worldState] * 
            GetMeasurementProbability(worldState, map, measurement));
      }
      return newBelief;
    }

    public abstract void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID);

    public abstract override string ToString();
}
}
